package com.example.shutdown_v1.parser;

import com.example.shutdown_v1.dto.SensorDataPacket;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Component;

@Slf4j
@Component
public class IM600Parser {
    
    // 转换系数 (±16g量程)
    private static final double COEF_ACCEL = 0.00478515625;
    private static final double COEF_GYRO = 0.06103515625;
    private static final double COEF_EULER = 0.005493164625;
    
    // 订阅ID位标志
    private static final int BIT0_ACCEL_NO_GRAVITY = 0x0001;
    private static final int BIT1_ACCEL_WITH_GRAVITY = 0x0002;
    private static final int BIT2_GYROSCOPE = 0x0004;
    private static final int BIT6_EULER_ANGLES = 0x0040;
    
    private static final double ROTATION_THRESHOLD = 5.0;
    
    public SensorDataPacket parse(String hexString, String mac) {
        try {
            // 数据校验
            if (hexString == null || hexString.trim().isEmpty()) {
                log.warn("[{}] hex数据为空", mac);
                return null;
            }
            
            hexString = hexString.replaceAll("\\s+", "");
            
            // 长度校验：IM600最小数据包应该至少包含包类型(1字节)+订阅ID(2字节)+时间戳(4字节) = 7字节 = 14个hex字符
            if (hexString.length() < 14) {
                log.warn("[{}] hex数据长度不足，期望>=14字符，实际={}字符: {}", mac, hexString.length(), hexString);
                return null;
            }
            
            byte[] data = hexStringToByteArray(hexString);
            
            if (data.length < 7) {
                log.error("数据包长度不足: {} bytes", data.length);
                return null;
            }
            
            SensorDataPacket packet = new SensorDataPacket();
            packet.setMac(mac);
            packet.setRawHexData(hexString);
            
            int offset = 0;
            packet.setPacketType(data[offset++]);
            
            int subscriptionId = readUInt16LE(data, offset);
            packet.setSubscriptionId(subscriptionId);
            offset += 2;
            
            long timestamp = readUInt32LE(data, offset);
            packet.setTimestamp(timestamp);
            offset += 4;
            
            // Bit0: 欧拉角圈数 (6字节) - 原无重力加速度位置
            // 只使用Roll和Yaw圈数，Pitch圈数跳过
            if ((subscriptionId & BIT0_ACCEL_NO_GRAVITY) != 0) {
                double rollTurnCount = readInt16LE(data, offset) * COEF_EULER;
                offset += 2;
                // 跳过Pitch圈数（不使用）
                offset += 2;
                double yawTurnCount = readInt16LE(data, offset) * COEF_EULER;
                offset += 2;
                
                packet.setRollTurnCount(rollTurnCount);
                packet.setYawTurnCount(yawTurnCount);
                
                // 注意：综合圈数需要在状态识别服务中计算差异值，这里不计算
                
                log.debug("[{}] 欧拉角圈数: Roll={:.3f}圈, Yaw={:.3f}圈",
                    mac, rollTurnCount, yawTurnCount);
            }
            
            // Bit1: 含重力加速度 (6字节)
            if ((subscriptionId & BIT1_ACCEL_WITH_GRAVITY) != 0) {
                int rawX = readInt16LE(data, offset);
                offset += 2;
                int rawY = readInt16LE(data, offset);
                offset += 2;
                int rawZ = readInt16LE(data, offset);
                offset += 2;
                
                double accelX = rawX * COEF_ACCEL;
                double accelY = rawY * COEF_ACCEL;
                double accelZ = rawZ * COEF_ACCEL;
                
                packet.setAccelX(accelX);
                packet.setAccelY(accelY);
                packet.setAccelZ(accelZ);
                
                if (log.isDebugEnabled()) {
                    double magnitude = Math.sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);
//                    log.debug("[{}] Accel with gravity: raw=[0x{} 0x{} 0x{}], value=[{:.4f}, {:.4f}, {:.4f}]m/s², mag={:.4f}",
//                        mac,
//                        Integer.toHexString(rawX & 0xFFFF).toUpperCase(),
//                        Integer.toHexString(rawY & 0xFFFF).toUpperCase(),
//                        Integer.toHexString(rawZ & 0xFFFF).toUpperCase(),
//                        accelX, accelY, accelZ, magnitude);
                }
            }
            
            if ((subscriptionId & BIT2_GYROSCOPE) != 0) {
                double gyroX = readInt16LE(data, offset) * COEF_GYRO;
                offset += 2;
                double gyroY = readInt16LE(data, offset) * COEF_GYRO;
                offset += 2;
                double gyroZ = readInt16LE(data, offset) * COEF_GYRO;
                offset += 2;
                
                packet.setGyroX(gyroX);
                packet.setGyroY(gyroY);
                packet.setGyroZ(gyroZ);
                
                double maxGyro = Math.max(Math.abs(gyroX), 
                                 Math.max(Math.abs(gyroY), Math.abs(gyroZ)));
                packet.setIsRotating(maxGyro > ROTATION_THRESHOLD);
            }
            
            // 跳过其他字段
            if ((subscriptionId & 0x0008) != 0) offset += 6;  // Bit3: 磁场
            if ((subscriptionId & 0x0010) != 0) offset += 8;  // Bit4: 温度/气压/高度
            if ((subscriptionId & 0x0020) != 0) offset += 8;  // Bit5: 四元数
            
            // Bit6: 欧拉角 (6字节)
            if ((subscriptionId & BIT6_EULER_ANGLES) != 0) {
                double roll = readInt16LE(data, offset) * COEF_EULER;
                offset += 2;
                double pitch = readInt16LE(data, offset) * COEF_EULER;
                offset += 2;
                double yaw = readInt16LE(data, offset) * COEF_EULER;
                offset += 2;
                
                packet.setPitch(pitch);
                packet.setRoll(roll);
                packet.setYaw(yaw);
                
                log.debug("[{}] Euler angles: Roll={:.2f}, Pitch={:.2f}, Yaw={:.2f}",
                    mac, roll, pitch, yaw);
            }
            
            return packet;
            
        } catch (Exception e) {
            log.error("数据解析失败: {}", e.getMessage(), e);
            return null;
        }
    }
    
    private byte[] hexStringToByteArray(String hex) {
        int len = hex.length();
        if (len % 2 != 0) {
            throw new IllegalArgumentException("Invalid hex string length: " + len);
        }
        byte[] data = new byte[len / 2];
        for (int i = 0; i < len; i += 2) {
            data[i / 2] = (byte) ((Character.digit(hex.charAt(i), 16) << 4)
                                + Character.digit(hex.charAt(i + 1), 16));
        }
        return data;
    }
    
    private short readInt16LE(byte[] data, int offset) {
        int low = data[offset] & 0xFF;
        int high = data[offset + 1] & 0xFF;
        int unsigned = (high << 8) | low;
        if (unsigned > 32767) {
            return (short) (unsigned - 65536);
        }
        return (short) unsigned;
    }
    
    private int readUInt16LE(byte[] data, int offset) {
        int low = data[offset] & 0xFF;
        int high = data[offset + 1] & 0xFF;
        return (high << 8) | low;
    }
    
    private long readUInt32LE(byte[] data, int offset) {
        long byte0 = data[offset] & 0xFF;
        long byte1 = data[offset + 1] & 0xFF;
        long byte2 = data[offset + 2] & 0xFF;
        long byte3 = data[offset + 3] & 0xFF;
        return (byte3 << 24) | (byte2 << 16) | (byte1 << 8) | byte0;
    }
}

